#ifndef __PURE_SUIT_H__
#define __PURE_SUIT_H__

#include <mc_common/base_define.h>
#include <mc_common/msg_convert.h>
#include <mc_common/math_utils.h>
#include <mc_common/path_utils.h>

class PureSuit{
    public:
        PureSuit(){
            kpLinear_ = 0.5;
            maxLinear_ = 1.0;
            maxAngular_ = 2.0;
        }

        // 设置速度限制
        void setVelocityLimit(float max_linear, float max_angular){
            maxLinear_ = max_linear;
            maxAngular_ = max_angular;
        }

        // 计算局部目标点的速度
        void ComputeTwist(Velocity2D curr_vel, Velocity2D& next_vel, Pose2D local_goal){
            float dis = hypot(local_goal[0], local_goal[1]);
            float ang = atan2(local_goal[1], local_goal[0]);
            float alpha = math_utils::DifferenceAngle(0.0, ang);
            float beta = math_utils::DifferenceAngle(ang, local_goal[2]);

            // 角加速度定义
            float angle_acceleration = 0.5;

            // 如果角度偏差过大，优先旋转处理
            float goal_error = ang;
            if(fabs(goal_error) > math_utils::Deg2Rad(20) && dis > 0.2){
                next_vel[0] = 0.0;
                next_vel[1] = 0.0;
                // 最大减速 v^2 = v0^2 + 2as
                float max_speed_to_stop = sqrt(2 * angle_acceleration * fabs(goal_error));
                next_vel[2] = goal_error > 0 ? max_speed_to_stop : -max_speed_to_stop;
            }
            else{
                // 机器人移动到目标点，先旋转alpha角度指向目标点，在移动dis距离，最后再旋转betha角度。
                // 权重先后算法函数
                // kp_alpha 机器人角度 与 机器人指向目标点角度 差
                // kp_betha 目标点角度 与 机器人指向目标点角度 差
                // 角速度评估函数 w = kp_alpha * alpha * dis + kp_beta * beta
                // kp_aplha、kp_beta为权重系数，kp_aplha > kp_beta  旋转alpha角度比旋转betha角度优先级更高，避免偏离目标点
                // 当离目标点dis越来越近时，第一项kp_alpha * alpha * dis影响越来越小
                // next_vel[2] = kpAlpha_ * alpha * dis + kpBeta_ * beta;

                if(dis < 0.1){  //抵达目标点，开始旋转到指定角度     
                    if(fabs(local_goal[2]) > math_utils::Deg2Rad(5)){
                        next_vel[0] = 0.0;
                        next_vel[1] = 0.0;
                        // 最大减速 v^2 = v0^2 + 2as
                        float max_speed_to_stop = sqrt(2 * angle_acceleration * fabs(local_goal[2]));
                        next_vel[2] = local_goal[2] > 0 ? max_speed_to_stop : -max_speed_to_stop;
                    }                        
                    else{
                        next_vel[0] = 0.0;
                        next_vel[1] = 0.0;  
                        next_vel[2] = 0.0;                        
                    }
                }
                else{
                    // 距离越远，速度越快
                    next_vel[0] = kpLinear_ * dis;  // 简单的PID控制
                    next_vel[1] = 0.0;
                    next_vel[2] = 2.0 * next_vel[0] * sin(alpha) / dis;
                }
            }

            cout << "[ComputeTwist] dis: " << dis
                << ", ang: " << ang
                << ", alpha: " << alpha
                << ", beta: " << beta
                << ", next_vel: " << msg_convert::Velocity2D2Str(next_vel) 
                << ", goal_error: " << goal_error
                << ", deg_20: " << math_utils::Deg2Rad(20)
                << endl;

            // 速度限制
            next_vel[0] = max(min(maxLinear_, next_vel[0]), -maxLinear_);   
            next_vel[1] = 0.0;
            next_vel[2] = max(min(maxAngular_, next_vel[2]), -maxAngular_);
        }

    private:
        float kpLinear_;   //线速度
        float maxLinear_;
        float maxAngular_;      
};


#endif